#include "main.h" // Device header
#include "DriversStruct.h"

/*PID初始化*/

void PID_Init(void)
{
    /*  电机1  */
    Platform.motor1.g_Angle_pid.Setpoint = 0;           /* 设定目标值 */
    Platform.motor1.g_Angle_pid.ActualValue = 0.0;      /* 期望输出值 */
    Platform.motor1.g_Angle_pid.SumError = 0.0;         /* 积分值 */
    Platform.motor1.g_Angle_pid.Error = 0.0;            /*e[n]*/
    Platform.motor1.g_Angle_pid.LastError = 0.0;        /*e[n-1]*/
    Platform.motor1.g_Angle_pid.PrevError = 0.0;        /*e[n-2]*/
    Platform.motor1.g_Angle_pid.Proportion = Angle_KP1; /* 比例常数  */
    Platform.motor1.g_Angle_pid.integral = Angle_KI1;   /* 积分常数 */
    Platform.motor1.g_Angle_pid.Derivation = Angle_KD1; /* 微分常数  */
    Platform.motor1.g_Angle_pid.Pidoutput = 0.0;        /*传统PID的输出*/

    // 新增前馈参数初始化
    Platform.motor1.g_Angle_pid.Kv = Angle_Kv1; // 示例值
    Platform.motor1.g_Angle_pid.Ka = Angle_Ka1; // 示例值
    Platform.motor1.g_Angle_pid.LastSetpoint = 0;
    Platform.motor1.g_Angle_pid.PrevSetpoint = 0;
    Platform.motor1.g_Angle_pid.last_time = HAL_GetTick() - 1; // 防止首次dt=0

    Platform.motor1.g_Speed_pid.Setpoint = 0;           /* 设定目标值 */
    Platform.motor1.g_Speed_pid.ActualValue = 0.0;      /* 期望输出值 */
    Platform.motor1.g_Speed_pid.SumError = 0.0;         /* 积分值 */
    Platform.motor1.g_Speed_pid.Error = 0.0;            /*e[n]*/
    Platform.motor1.g_Speed_pid.LastError = 0.0;        /*e[n-1]*/
    Platform.motor1.g_Speed_pid.PrevError = 0.0;        /*e[n-2]*/
    Platform.motor1.g_Speed_pid.Proportion = Speed_KP1; /* 比例常数  */
    Platform.motor1.g_Speed_pid.integral = Speed_KI1;   /* 积分常数 */
    Platform.motor1.g_Speed_pid.Derivation = Speed_KD1; /* 微分常数  */
    Platform.motor1.g_Speed_pid.Pidoutput = 0.0;        /*传统PID的输出*/

    /*  电机2  */
    Platform.motor2.g_Angle_pid.Setpoint = 0;           /* 设定目标值 */
    Platform.motor2.g_Angle_pid.ActualValue = 0.0;      /* 期望输出值 */
    Platform.motor2.g_Angle_pid.SumError = 0.0;         /* 积分值 */
    Platform.motor2.g_Angle_pid.Error = 0.0;            /*e[n]*/
    Platform.motor2.g_Angle_pid.LastError = 0.0;        /*e[n-1]*/
    Platform.motor2.g_Angle_pid.PrevError = 0.0;        /*e[n-2]*/
    Platform.motor2.g_Angle_pid.Proportion = Angle_KP2; /* 比例常数  */
    Platform.motor2.g_Angle_pid.integral = Angle_KI2;   /* 积分常数 */
    Platform.motor2.g_Angle_pid.Derivation = Angle_KD2; /* 微分常数  */
    Platform.motor2.g_Angle_pid.Pidoutput = 0.0;        /*传统PID的输出*/

    // 新增前馈参数初始化
    Platform.motor2.g_Angle_pid.Kv = Angle_Kv2; // 示例值
    Platform.motor2.g_Angle_pid.Ka = Angle_Ka2; // 示例值
    Platform.motor2.g_Angle_pid.LastSetpoint = 0;
    Platform.motor2.g_Angle_pid.PrevSetpoint = 0;
    Platform.motor2.g_Angle_pid.last_time = HAL_GetTick() - 1; // 防止首次dt=0

    Platform.motor2.g_Speed_pid.Setpoint = 0;           /* 设定目标值 */
    Platform.motor2.g_Speed_pid.ActualValue = 0.0;      /* 期望输出值 */
    Platform.motor2.g_Speed_pid.SumError = 0.0;         /* 积分值 */
    Platform.motor2.g_Speed_pid.Error = 0.0;            /*e[n]*/
    Platform.motor2.g_Speed_pid.LastError = 0.0;        /*e[n-1]*/
    Platform.motor2.g_Speed_pid.PrevError = 0.0;        /*e[n-2]*/
    Platform.motor2.g_Speed_pid.Proportion = Speed_KP2; /* 比例常数  */
    Platform.motor2.g_Speed_pid.integral = Speed_KI2;   /* 积分常数 */
    Platform.motor2.g_Speed_pid.Derivation = Speed_KD2; /* 微分常数  */
    Platform.motor2.g_Speed_pid.Pidoutput = 0.0;        /*传统PID的输出*/
}

float Pid_closed_control(FeedForwardPID_TypeDef *PID, float Feedback_value)
{
    PID->Error = (float)(PID->Setpoint - Feedback_value); // 误差等于目标值-实际值

#if SELECT // 增量式
    PID->Pidoutput += (PID->Proportion * (PID->Error - PID->LastError)) + (PID->integral * PID->Error) + (PID->Derivation * (PID->Error - 2 * PID->LastError + PID->PrevError));

    PID->PrevError = PID->LastError;
    PID->LastError = PID->Error;
#else // 位置式

    PID->SumError += PID->Error; // 误差累计
    if (PID->SumError >= 10000)
    {
        PID->SumError = 10000;
    }
    if (PID->SumError <= -10000)
    {
        PID->SumError = -10000;
    }

    PID->ActualValue = (PID->Proportion * PID->Error) + (PID->integral * PID->SumError) + (PID->Derivation * (PID->Error - PID->LastError));
    PID->LastError = PID->Error;
#endif

    return ((float)(PID->ActualValue));
}

/*PID闭环控制*/

float FeedForwardPid_closed_control(FeedForwardPID_TypeDef *PID, float Feedback_value) // Feedback_value实际值
{
    // 1. 计算时间间隔（自动处理时间戳）
    uint32_t now = HAL_GetTick();
    float dt = (float)(now - PID->last_time) / 1000.0f; // 转换为秒

    // 处理首次调用和极小时间间隔
    if (dt <= 0.001f)
    {                             // 最小时间间隔保护
        dt = 0.001f;              // 1ms最小时间间隔
        PID->last_time = now - 1; // 维持有效时间差
    }
    else
    {
        PID->last_time = now; // 更新时间戳
    }

    // 2. 计算目标速度与加速度（前馈量）
    float target_vel = (PID->Setpoint - PID->LastSetpoint) / dt;
    float target_acc = (PID->Setpoint - 2 * PID->LastSetpoint + PID->PrevSetpoint) / (dt * dt);

    // 3. 更新目标位置历史记录
    PID->PrevSetpoint = PID->LastSetpoint;
    PID->LastSetpoint = PID->Setpoint;

    PID->Error = (float)(PID->Setpoint - Feedback_value); // 误差等于目标值-实际值

#if SELECT // 增量式
    PID->Pidoutput += (PID->Proportion * (PID->Error - PID->LastError)) + (PID->integral * PID->Error) + (PID->Derivation * (PID->Error - 2 * PID->LastError + PID->PrevError));

    PID->PrevError = PID->LastError;
    PID->LastError = PID->Error;
#else // 位置式

    PID->SumError += PID->Error; // 误差累计
    if (PID->SumError >= 10000)
    {
        PID->SumError = 10000;
    }
    if (PID->SumError <= -10000)
    {
        PID->SumError = -10000;
    }

    PID->Pidoutput = (PID->Proportion * PID->Error) + (PID->integral * PID->SumError) + (PID->Derivation * (PID->Error - PID->LastError));
    PID->LastError = PID->Error;
#endif

    // 5. 前馈补偿计算
    float feedforward = PID->Kv * target_vel + PID->Ka * target_acc;

    // 6. 总输出 = PID输出 + 前馈
    PID->ActualValue = PID->Pidoutput + feedforward;

    return ((float)(PID->ActualValue));
}
